#include "ros/ros.h"
#include "sf9DOFrazor/RPY.h"
#include "sf9DOFrazor/Serial.h"

#define MAX_READ_LEN 100

const char *PORT = "/dev/ttyUSB0"; //"/dev/tty.usbserial-A800cCE8";
const int   BAUD = 57600;

char zeroes[MAX_READ_LEN];

int main (int argc, char **argv) {
  ros::init(argc, argv, "sf9DOFrazor");
  
  LightweightSerial *conn = new LightweightSerial(PORT, BAUD);
  
  if (conn->is_ok() != true) {
    return 1;
  }
  
  for (int i = 0; i < MAX_READ_LEN; ++i)
    zeroes[i] = 0;
  
  ros::NodeHandle n;
  ros::Publisher imu_data_pub = n.advertise<sf9DOFrazor::RPY>("imu_data", 1000);
  
  std::vector<char> buf;
  buf.assign(zeroes, zeroes + sizeof(zeroes) / sizeof(char) );
  
  sf9DOFrazor::RPY msg;
  
  msg.pitch = 0.0;
  msg.roll  = 0.0;
  msg.yaw   = 0.0;
  
  while (ros::ok()) {
    if (conn->read_block((uint8_t *)&(buf .at(0)), MAX_READ_LEN) > 0) {
      if (sscanf((const char*)&(buf.at(0)), "!ANG:%7f,%7f,%7f", &msg.pitch, &msg.roll, &msg.yaw)) {
        ROS_DEBUG("%g, %g, %g\n", msg.pitch, msg.roll, msg.yaw);
        ROS_INFO("New Imu Readings:");
        ROS_INFO("  Pitch: %g", msg.pitch);
        ROS_INFO("  Roll:  %g", msg.roll);
        ROS_INFO("  Yaw:   %g", msg.yaw);
        
      } else {
        ROS_DEBUG("%s",(const char*)&(buf.at(0)));
      }
      buf.assign(zeroes, zeroes + sizeof(zeroes) / sizeof(char) );
    }
    
    ROS_DEBUG("Current Imu Readings:");
    ROS_DEBUG("  Pitch: %g", msg.pitch);
    ROS_DEBUG("  Roll:  %g", msg.roll);
    ROS_DEBUG("  Yaw:   %g", msg.yaw);
    
    imu_data_pub.publish(msg);
    
    ros::spinOnce();
  }

  delete conn;
  
  return 0;
}
